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ABSTRACT 

Aircraft  navigation  information  (position,  velocity,  and  at¬ 
titude)  can  be  determined  using  optical  measurements  from 
imaging  sensors  combined  with  an  inertial  navigation  sys¬ 
tem.  This  can  be  accomplished  by  tracking  the  locations  of 
optical  features  in  multiple  images  and  using  the  resulting 
geometry  to  estimate  and  remove  inertial  errors. 

A  critical  factor  governing  the  performance  of  optical- 
inertial  navigation  systems  is  the  robustness  of  the  feature 
tracking  algorithm.  Robust  feature  tracking  research  has 
focused  on  developing  multi-dimensional  feature  transfor¬ 
mations  which  are  invariant  to  camera  pose  variations.  In 
addition,  significant  effort  has  been  placed  into  algorithms 
designed  to  pair  features  between  images  from  large  sets 
(e.g.,  RANSAC).  This  ttaditional  approach  requires  large 
computational  resources,  especially  when  presented  with 
imaging  situations  with  sparse,  partially  obscured,  or  repet¬ 
itive  features. 

In  this  paper,  the  method  of  multi-dimensional  stochas¬ 
tic  constraints  is  applied  to  the  optical-inertial  navigation 


problem  in  two  dimensional  feature  space.  The  resulting 
navigation  system  uses  inertial  measurements  to  aid  the 
feature  ttacking  algorithm,  which  results  in  improvements 
in  robustness  and  processing  speed.  The  performance  of 
the  optical-inertial  navigation  system  is  demonsttated  us¬ 
ing  experimental  data. 

INTRODUCTION 

Motivation 

The  benefits  of  tightly  integrating  navigation  sensors,  such 
as  inertial  measurement  units  (IMU)  and  global  position¬ 
ing  system  (GPS)  receivers,  is  well-known.  The  compli¬ 
mentary  characteristics  of  the  two  sensors  allow  the  inte¬ 
grated  system  to  perform  at  levels  which  are  difficult  to 
attain  with  either  sensor  alone  (see  [2]).  As  a  result,  in¬ 
tegrated  IMU/GPS  systems  have  become  common,  espe¬ 
cially  in  military-grade  navigation  systems.  Unfortunately, 
GPS  signals  are  not  available  in  all  locations,  which  moti¬ 
vates  the  development  of  a  non-GPS  based  navigation  ref¬ 
erence  which  can  aid  an  inertial  navigation  system. 

One  non-GPS  navigation  approach  is  to  integrate  a  camera 
with  an  inertial  sensor.  This  technique  has  some  impor¬ 
tant  advantages.  Foremost,  the  sensors  can  operate  in  en¬ 
vironments  where  GPS  is  difficult  to  receive  (e.g.,  indoors, 
under  ttees,  underwater,  etc.).  Secondly,  the  sensors  are 
completely  passive  and  do  not  require  the  transmission  (or 
reception)  of  radio  signals.  Finally,  optical  and  inertial  sen¬ 
sors  are  immune  to  disruptions  in  the  radio  spectrum. 

The  development  of  low-cost  inertial  and  optical  sensors 
has  led  to  remarkable  advances  in  the  field  of  optical-aided 
navigation  [15,  18-20],  In  these  systems,  digital  images 
are  combined  with  inertial  measurements  to  estimate  posi¬ 
tion,  velocity,  and  attitude.  In  this  paper,  a  novel,  tightly- 
integrated  optical-inertial  navigation  system  is  developed, 
based  on  the  theory  of  stochastic  projections.  The  system 
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is  tested  with  both  simulated  and  experimental  data.  The 
performance  of  the  system  and  potential  areas  for  contin¬ 
ued  research  are  discussed.  This  effort  is  part  of  ongoing 
research  into  fusion  of  optical  and  inertial  sensors  for  long¬ 
term  autonomous  navigation. 

Current  Methods 

It  is  well-known  that  optical  measurements  provide  ex¬ 
cellent  navigation  information,  when  interpreted  properly. 
Optical  navigation  is  not  new.  Pilotage  is  the  oldest  and 
most  natively  familiar  form  of  navigation  to  humans  and 
other  animals.  For  centuries,  navigators  have  utilized  me¬ 
chanical  instruments  such  as  astrolabes,  sextants,  and  drift- 
meters  [15]  to  make  precision  observations  of  the  sky  and 
ground  in  order  to  determine  their  position,  velocity,  and 
attitude. 

The  difficulty  in  using  optical  measurements  for  au¬ 
tonomous  navigation  (that  is,  without  human  intervention) 
has  always  been  in  the  interpretation  of  the  image,  a  diffi¬ 
culty  shared  with  Automatic  Target  Recognition  (ATR).  In¬ 
deed,  when  celestial  observations  are  used,  the  ATR  prob¬ 
lem  in  this  structured  environment  is  tractable,  and  auto¬ 
matic  star  trackers  are  widely  used  for  space  navigation  and 
ICBM  guidance  [7, 16, 17].  When  ground  images  are  to  be 
used,  the  difficulties  associated  with  image  interpretation 
are  paramount.  At  the  same  time,  the  problems  associated 
with  the  use  of  optical  measurements  for  navigation  are 
somewhat  easier  than  ATR.  Moreover,  recent  developments 
in  feature  tracking  algorithms,  miniaturization,  and  reduc¬ 
tion  in  cost  of  inertial  sensors  and  optical  imagers,  aided  by 
the  continuing  improvement  in  microprocessor  technology, 
motivates  the  use  of  inertial  measurements  to  aid  the  task 
of  feature  tracking  in  image  sequences. 

Image-aiding  methods  are  typically  classified  as  either 
feature-based  or  optic  flow-based,  depending  on  how  the 
image  correspondence  problem  is  addressed.  Feature- 
based  methods  determine  correspondence  for  “landmarks” 
in  the  scene  over  multiple  frames,  while  optic  flow-based 
methods  typically  determine  correspondence  for  a  whole 
portion  of  the  image  between  frames.  A  good  reference  on 
image  correspondence  is  [10].  Optic  flow  methods  have 
been  proposed  generally  for  elementary  motion  detection, 
focusing  on  determining  relative  velocity,  angular  rates,  or 
obstacle  avoidance  [5], 

Feature  tracking-based  navigation  methods  have  been  pro¬ 
posed  both  for  fixed-mount  imaging  sensors  or  gimbal 
mounted  detectors  which  “stare”  at  the  target  of  interest, 
in  a  manner  similar  to  the  gimballed  infrared  seeker  on 
heat-seeking,  air-to-air  missiles.  Many  feature  tracking- 
based  navigation  methods  exploit  knowledge  (either  a  pri¬ 
ori,  through  binocular  stereopsis,  or  by  exploiting  terrain 
homography)  of  the  target  location  and  solve  the  inverse 


trajectory  projection  problem  [1, 13].  If  no  a  priori  knowl¬ 
edge  of  the  scene  is  provided,  egomotion  estimation  is 
completely  correlated  with  estimating  the  scene.  This  is  re¬ 
ferred  as  the  structure  from  motion  (SFM)  problem.  A  the¬ 
oretical  development  of  the  geometry  of  fixed-target  track¬ 
ing,  with  no  a  priori  knowledge  is  provided  in  [14],  An  on¬ 
line  (Extended  Kalman  Filter-based)  method  for  calculat¬ 
ing  a  trajectory  by  tracking  features  at  an  unknown  location 
on  the  Earth’s  surface,  provided  the  topography  is  known  is 
given  in  [4],  Finally,  navigation-grade  inertial  sensors  and 
terrain  images  collected  on  a  T-38  “Talon”  were  processed 
and  the  potential  benefits  of  optical-aided  inertial  sensors 
are  experimentally  demonstrated  in  [18]. 

Many  methods  for  solving  the  correspondence  problem 
have  been  proposed  in  the  computer  vision  literature.  A 
popular  algorithm  is  the  Lucas-Kanade  feature  tracker  [9], 
which  relies  on  the  premise  of  the  invariance  of  the  in¬ 
tensity  field  between  images.  It  uses  a  template  correla¬ 
tion  algorithm  to  minimize  the  sum  of  squared  differences 
(SSD)  between  image  intensities.  The  algorithm  typically 
assumes  a  linear  (x  —  y  plane)  motion  model,  but  can 
be  extended  to  optimize  over  affine  or  bilinear  transfor¬ 
mations  [9,21],  Other  feature  correspondence  algorithms 
have  been  proposed  which  are  invariant  to  rotations,  scal¬ 
ing  or  both  [8].  More  robust  feature  tracking  algorithms  are 
typically  computationally  expensive,  and  a  designer  must 
trade  tracking  robustness  and  accuracy  for  real-time  per¬ 
formance  [21], 

In  this  paper,  the  method  of  stochastic  projections  [24]  is 
used  as  the  basis  for  tightly  integrating  an  inertial  and  op¬ 
tical  sensor  using  an  Extended  Kalman  Filter  (EKF)  and 
an  automatic  target  tracking  algorithm.  In  the  following 
section,  the  integration  architecture  is  presented,  which  in¬ 
cludes  the  underlying  assumptions,  the  inertial  mechaniza¬ 
tion  algorithms,  EKF  state  model,  measurement  model,  and 
feature  tracking  concept. 

DEVELOPMENT 

The  method  proposed  in  this  paper  employs  an  extended 
Kalman  filter  (EKF)  algorithm  [11, 12]  to  recursively  esti¬ 
mate  the  navigation  state  and  associated  errors  by  tracking 
the  pixel  locations  of  stationary  objects  in  an  image-aided 
inertial  system. 


Assumptions 

This  method  is  based  on  the  following  assumptions. 

•  A  strapdown  inertial  measurement  unit  (IMU)  is 
rigidly  attached  to  one  or  more  cameras.  Synchro¬ 
nized  raw  measurements  are  available  from  both  sen¬ 
sors. 

•  The  camera  produces  images  of  objects  which  are  sta¬ 
tionary  (or  very  slowly  moving)  with  respect  to  the 
world. 

•  Some  form  of  range  to  landmark  measurement  is 
available,  either  through  binocular  stereopsis  or  using 
a  statistical  terrain  model. 

•  The  inertial  and  optical  sensors’  relative  position  and 
orientation  is  known  (see  [23]  for  a  discussion  of  bore- 
sight  procedures). 


Optical  Sensor  Model 

An  optical  sensor  is  a  device  designed  to  measure  the  in¬ 
tensity  of  optical  energy  (light)  entering  the  sensor  through 
an  aperture.  Imaging  sensors  consist  of  an  array  of  light- 
sensitive  detectors  which  create  a  multidimensional  light 
intensity  measurement  (i.e.,  image).  In  this  section,  the  ba¬ 
sic  physical  properties  of  an  optical  sensor  are  presented, 
and  a  model  representing  an  optical  sensor  is  given. 

For  the  purposes  of  this  discussion,  the  world  is  defined  as 
a  collection  of  real  objects.  Some  objects  are  sources  of 
radiometric  illumination  or  radiance.  These  light  sources 
illuminate  the  world  and  interact  with  the  other  physical 
objects  through  various  types  of  reflection.  The  amount 
of  light  along  a  certain  direction  is  defined  as  the  irra- 
diance  [10],  The  physical  irradiance  pattern  entering  the 
aperture  of  the  optical  sensor  is  defined  as  the  scene  and  is 
represented  by  a  continuous  array  of  nonnegative  real  num¬ 
bers,  o(x,y,t),  projected  onto  the  image  plane.  For  the 
purposes  of  this  discussion,  the  irradiance  sources  are  con¬ 
strained  to  an  arbitrary,  piecewise  continuous,  Lambertian 
surface  in  three  dimensions. 

A  digital  optical  imaging  sensor  consists  of  an  aperture, 
lens,  detector  array,  and  sampling  array.  A  simple  imaging 
system  model  is  shown  in  Figure  1.  The  lens  focuses  the 
scene  on  the  detector  array.  The  light  pattern  focused  on  the 
detector  array  is  defined  as  the  image  and  represented  by, 
i(cc,  y,  t).  In  statistical  terms,  the  image  is  the  mean  photon 
arrival  rate,  and  is  well-modeled  using  a  Poisson  distribu¬ 
tion  [3].  The  detector  array  converts  the  light  energy  into  a 
voltage  or  a  charge  which  is  converted  to  a  digital  value  by 
the  sampling  array.  The  sampling  array  is  assumed  to  be  a 


ILLUMINATION 


Figure  1:  Imaging  system  model.  The  imaging  system 
transforms  the  scene  into  a  digital  image.  The  major  com¬ 
ponents  of  the  camera  are  the  optics,  light  detector,  ampli¬ 
fier,  and  analog  to  digital  converter. 

square  grid,  although  other  patterns  can  be  designed  (e.g., 
honeycomb)  [6]. 

The  images  are  transformed  into  the  feature  space,  which  is 
described  in  the  next  section.  The  pixel  location  of  feature 
m  in  the  image  at  time  is  given  by  the  vector,  z m(L). 
This  pixel  location  corresponds  to  a  line-of-sight  vector 
which  extends  outward  from  the  camera  focus.  This  ho¬ 
mogeneous  line-of-sight  vector,  is  calculated  using 

the  linear  projection  determined  during  camera  calibration, 

s  cm(u)  =  t  r  [z  M  (i) 

where  z m(L)  has  been  corrected  for  non-linear  distortion 
effects  of  the  imaging  system. 

Feature  Model 

To  ensure  robust,  long-term  feature  tracking,  much  effort  is 
placed  upon  extracting  features  which  are  invariant  to  cam¬ 
era  motion.  This  motion  is  observed  as  changes  in  scale, 
rotation,  translation,  and  affine  transformation  of  the  im¬ 
age.  It  is  interesting  to  note  the  coupled  nature  between 
camera  motion  and  image  changes.  When  estimating  the 
camera  motion,  it  is  the  “change”  in  features  which  must  be 
observed.  Lowe’s  scale-invariant  (SIFT)  features  are  fully 
invariant  to  both  rotation  and  scale  and  partially  invariant  to 
affine  motion  [8].  These  invariant  properties  make  this  fea¬ 
ture  transformation  attractive  for  image-aided  navigation. 

Algorithm  Description 

The  system  parameters  (see  Table  1)  consist  of  the  nav¬ 
igation  parameters  (position,  velocity,  and  attitude),  iner¬ 
tial  sensor  biases,  and  a  vector  describing  the  location  of 
landmarks  of  interest  (y).  The  navigation  parameters  are 
calculated  using  body -frame  velocity  increment  (Avfc)  and 
angular  increment  (A 6bib)  measurements  from  the  inertial 


navigation  sensor  which  have  been  corrected  for  bias  er¬ 
rors  using  the  current  filter-computed  bias  estimates.  These 
measurements  are  integrated  from  an  initial  state  in  the  nav¬ 
igation  (local-level)  frame  using  mechanization  algorithms 
described  in  [22], 


Table  1:  System  Parameter  Definition 


Parameter 

Description 

pn 

Vehicle  position  in  navigation  frame 
(northing,  easting,  and  down) 

v” 

Vehicle  velocity  in  navigation  frame 
(north,  east,  down) 

(-in 
^ b 

Vehicle  body  to  navigation  frame  DCM 

a  6 

Acclerometer  bias  vector 

bb 

Gyroscope  bias  vector 

c 

Location  of  landmark  to  in  the 
navigation  frame  (one  for  each  landmark 
currently  tracked) 

db 

Camera-to-IMU  lever  arm  in  body  frame 

c* 

Camera-to-IMU  orientation  DCM 

An  Extended  Kalman  Filter  was  constructed  to  estimate  the 
errors  in  the  calculated  system  parameters.  In  order  to  min¬ 
imize  the  effects  of  linearization  errors,  the  system  parame¬ 
ters  were  periodically  corrected  by  removing  the  current  er¬ 
ror  estimate  [11].  A  block  diagram  of  the  system  is  shown 
in  Figure  2. 


Figure  2:  Optical-inertial  navigation  filter  block  diagram. 
In  this  filter,  the  location  of  stationary  objects  are  tracked 
and  used  to  estimate  and  update  the  errors  in  an  inertial 
navigation  system.  The  inertial  navigation  system  is,  in 
turn,  used  to  support  the  feature  tracking  loop. 


The  Kalman  filter  state  vector,  x,  is  defined  as 

‘  5pn  ' 

6vn 


Sbb 

.  £y  . 

where  Spn  is  the  estimated  position  error  vector,  Sv"  is 
the  estimated  velocity  error  vector,  and  if  is  the  estimated 
body-to-navigation  frame  attitude  error  (defined  as  small- 
angle  rotations  about  the  north,  east,  and  down  axes).  The 
accelerometer  and  gyroscope  bias  errors  are  represented  by 
5&b  and  6 bb,  respectively.  The  landmark  position  error  vec¬ 
tor  (6 y)  is  defined  as  a  collection  of  the  errors  (r)t”  j  in  the 
currently  tracked  landmark  positions.  The  perturbation  er¬ 
ror  states  are  defined  as 


<5p" 

=  pn  -  pn 

(3) 

<5v" 

=  v"-v" 

(4) 

rm 

=  [i-V>x]C£ 

(5) 

5ab 

=  ab  —  ab 

(6) 

<5bb 

=  bb  -  bb 

(7) 

<5y 

=  y-y 

(8) 

where  the  tilde  0  represents  the  nominal  (estimated)  tra¬ 
jectory. 

The  position,  velocity,  and  attitude  errors  were  modeled 
as  a  stochastic  process  based  on  the  well-known  Pinson 
navigation  error  model  [22],  The  accelerometer  and  gyro¬ 
scopic  bias  errors  were  each  modeled  as  a  first-order  Gauss- 
Markov  process  [11],  based  on  the  specification  for  the  in¬ 
ertial  measurement  unit  (IMU).  The  landmarks  are  mod¬ 
eled  as  stationary  with  respect  to  the  Earth.  A  small  amount 
of  process  noise  is  added  to  the  state  dynamics  to  promote 
filter  stability  [12], 

Landmark  Track  Maintenance 

In  a  practical  system,  the  number  of  Kalman  Filter  states 
is  limited  by  available  computer  resources.  As  a  result,  the 
number  of  landmarks  actively  tracked  must  be  constrained. 
This  inherent  limitation  motivates  the  implementation  of  a 
track  maintenance  algorithm. 

The  general  concept  for  the  track  maintenance  algorithm 
is  to  add  and  prune  landmark  tracks  in  order  to  provide 
the  “best”  navigation  information  to  the  filter.  Although 
the  optimal  landmark  choices  are  highly  dependent  on  the 
trajectory  and  scene,  some  general  “mles-of-thumb”  were 
used  in  the  track  maintenance  algorithm. 

In  general,  features  which  can  be  easily  and  accurately 
tracked  for  long  periods  of  time  provide  the  best  naviga¬ 
tion  information.  This  implies  choosing  features  which 


are:  strong  and  easily  identified  (to  help  maintain  track),  lo¬ 
cally  distinct  (to  eliminate  false  correspondence),  and  well- 
separated  in  image  space  (to  maximize  filter  observability). 
Thus,  when  Kalman  Filter  landmark  track  states  are  avail¬ 
able,  the  feature  space  of  the  current  image  is  searched  and 
new  landmarks  are  added  based  on  the  above  criteria.  The 
filter  states  are  augmented  in  accordance  with  the  stochas¬ 
tic  projection  algorithm  defined  in  [24]. 

In  order  to  maintain  only  the  best  tracks,  stale  landmark 
tracks  (i.e.,  no  successful  correspondence  available  for  a 
given  period  of  time)  are  pruned  by  removing  the  associ¬ 
ated  filter  states.  Other  track  maintenance  approaches  are 
possible  which  could  theoretically  improve  the  track  per¬ 
formance  (e.g.,  semi-causal,  multiple  model,  or  velocity 
prediction),  however  these  approaches  will  not  be  pursued 
in  this  paper. 

Stale  Landmark  Revisitation 

As  proposed  by  Strelow  in  [21],  the  navigation  perfor¬ 
mance  can  be  improved  considerably  when  the  system  can 
revisit  landmarks  which  were  previously  tracked.  This  ap¬ 
proach  could  theoretically  constrain  the  growth  of  naviga¬ 
tion  errors  indefinitely;  however,  it  would  not  be  of  bene¬ 
fit  for  long-distance  navigation.  This  approach  will  not  be 
considered  in  this  paper. 

Measurement  Model  In  order  to  exploit  the  synergistic 
properties  of  optical  and  inertial  sensors,  the  navigation  and 
feature  tracking  algorithms  are  tightly-coupled.  This  re¬ 
sults  in  a  slight  modification  to  the  standard  Kalman  filter 
update  and  propagation  cycles  in  order  to  incorporate  the 
feature  tracking  loop.  The  tracking  loop  is  responsible  for: 
incorporating  new  landmark  tracks,  using  stochastic  pro¬ 
jections  to  predict  and  match  features  between  images,  pro¬ 
viding  filter  measurements,  and  deleting  stale  landmarks 
from  the  filter. 

The  Kalman  filter  assists  the  tracking  algorithm  by  main¬ 
taining  and  propagating  the  minimum  mean-square  error 
state  estimate.  This  provides  the  stochastic  projections 
which  help  improve  the  speed  and  robustness  of  the  track¬ 
ing  loop. 

The  tracking  loop  incorporates  new  landmark  tracks  when 
necessary  by  determining  an  initial  estimate  of  the  land¬ 
mark  location  (using  either  a  terrain  model  or  binocu¬ 
lar  stereopsis  combined  with  the  current  navigation  state 
vector).  This  estimate,  along  with  the  calculated  covari¬ 
ance  and  cross-correlation  matrices  are  augmented  into  the 
Kalman  filter  state  vector  and  covariance  matrix.  Details 
on  the  mathematics  involved  in  the  calculation  of  the  above 
process  are  based  on  the  stochastic  projection  model  de¬ 
scribed  in  [24] . 


After  the  landmark  tracks  are  properly  augmented  into  the 
Kalman  filter,  the  standard  propagation  algorithms  are  used 
to  predict  the  augmented  state  to  the  time  of  the  next  image. 
The  location  of  each  landmark  (along  with  arbitrary  uncer¬ 
tainty  ellipsoid)  can  then  be  projected  into  the  feature  space 
of  the  new  image.  In  this  paper,  the  feature  space  corre¬ 
sponds  to  a  two-dimensional  pixel  location  and  associated 
uncertainty  ellipse.  The  tracking  algorithm  then  searches 
this  uncertainty  ellipse  for  a  feature  which  has  similar  char¬ 
acteristics  to  the  reference  feature  and  is  distinct.  In  this 
paper,  a  2  —  a  ellipse  was  used.  An  example  of  feature  pre¬ 
diction  is  shown  in  Figure  3.  The  reader  is  referred  to  [24] 
for  more  details  regarding  the  feature  prediction  algorithm. 


Figure  3:  Stochastic  feature  projection.  Optical  features 
of  interest  are  projected  into  future  images  using  inertial 
measurements  and  stochastic  projections. 

Once  the  landmark  tracker  has  determined  a  correspond¬ 
ing  match,  the  pixel  location  of  the  feature,  corrected  for 
optical  distortion,  is  used  to  update  the  navigation  state. 
The  measured  pixel  location  for  feature  m  is  described  as  a 
function  of  the  system  parameters  by 

zb(fi)=h[p",C^C,C^d6]  +  v(tj)  (9) 

or,  more  specifically. 


zb(U)  =  TPixs^(fj)  +  v(fi)  (10) 

where  is  the  linear  projection  matrix  (see  [10])  and 
s is  the  homogeneous  (i.e.,  unit  normalized  by  sz 
component)  line-of-sight  vector  to  landmark  m,  expressed 
in  the  camera  frame.  The  measurement  is  corrupted  by  v,  a 
zero-mean,  white,  Gaussian  noise  process  with  covariance 


E  [v(ti)v*(tj)] 


R  tj.  —  tj 

0  ti  tj 


(11) 


All  measurements  are  modeled  as  independent.  The  line- 
of-sight  vector  is  defined  as 

scm  =  CcbCbn  [C  -  p’1]  -  Cgd"  (12) 

where  Ch  and  dfc  are  the  camera  boresight  parameters. 


The  linearized  observation  matrix,  H,  is  the  Jacobian  of  the 
nonlinear  measurement  function,  h[-],  linearized  about  the 
reference  trajectory,  x: 


H  = 


(13) 


The  partial  derivative  with  respect  to  position  is  expressed 


as 
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(14) 

where. 
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and, 

dp «  “ 

(16) 

The  partial  derivative  with  respect  to  body-to-navigation 
frame  misalignment  angle  vector,  ip,  is 

9h  TpiXdscm 

dip  c  dip 

(17) 

where 

dscm  c  ds°m 

^—m  d'ip  —m  Q'lfy 
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(18) 

and 

f)  sc 

-g^  =  -CcbCbn  [(t"-p")x] 

(19) 

The  skew  symmetric  operator,  (•)  x,  is  defined  for  a  generic 
3x3  vector,  v  as 
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Finally,  the  partial  derivative  with  respect  to  the  landmark 
position  vector,  t^,  is 
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Figure  4:  Data  collection  system.  The  data  collection  sys¬ 
tem  consisted  of  a  tactical-grade  IMU  and  monochrome 
digital  cameras. 


All  other  partial  derivatives  are  zero. 
The  resulting  observation  matrix  is 


H  = 


dh 
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calculated  for  all  successful  feature 
matches  at  the  current  time. 


ah 

<9t^ 

(24) 

correspondence 


SYSTEM  TESTS 

The  data  collection  system  consisted  of  an  IMU  and  two 
digital  cameras  (see  Figure  4).  The  IMU  was  a  Honey¬ 
well  HG-1700  tactical-grade  unit  which  measured  acceler¬ 
ation  and  angular  rate  at  100  Hz.  The  digital  cameras  were 
both  Pixelink  A-741  machine  vision  cameras  which  incor¬ 
porated  a  global  shutter  feature  and  a  Firewire  interface. 
The  lenses  were  wide-angle  Pentax  lenses  with  approxi¬ 
mately  90  degrees  held  of  view.  The  sensors  were  mounted 
on  an  aluminum  plate  and  calibrated  using  procedures  sim¬ 
ilar  to  those  described  in  [23].  Images  were  captured  at 
approximately  1  Hz. 

The  algorithm  was  tested  experimentally  using  two  naviga¬ 
tion  profiles  designed  to  examine  the  sensitivity  and  robust¬ 
ness  of  the  feature  tracking  system.  The  first  profile  con¬ 
sisted  of  a  closed  path  over  an  outdoor  parking  area.  The 
path  was  traversed  forward  and  backwards  with  the  camera 
pointed  toward  the  outside  of  the  path.  This  trajectory  re¬ 
sulted  in  seven  segments  that  presented  a  scene  change  and 
forced  the  filter  to  search  for  new  features.  This  outdoor 
scene  consisted  of  a  combination  of  man-made  features 
(buildings,  fences,  roads,  etc.)  and  natural  features  such 


Figure  5:  Sample  image  from  outdoor  data  collection.  The 
outdoor  data  collection  presented  the  filter  with  a  combi¬ 
nation  of  man-made  and  natural  features.  The  crosses  and 
ellipses  indicate  the  locations  and  uncertainty  of  currently 
tracked  landmarks. 

as  grass  and  trees.  The  profile  consisted  of  a  10-minute  sta¬ 
tionary  alignment  period,  followed  by  four  minutes  of  nav¬ 
igation  using  only  images  and  inertial  measurements.  No 
prior  knowledge  was  used  with  any  feature.  The  filter  was 
limited  to  a  maximum  of  ten  features  at  any  time.  A  sample 
image  from  the  outdoor  profile  is  shown  in  Figure  5. 

This  profile  presented  the  algorithm  with  a  challenging  fea¬ 
ture  tracking  environment  due  to  the  high-contrast  lighting 
conditions,  large  variation  in  feature  distance  (zero  to  in¬ 
finite),  and  complicated  images  with  semi-transparent  ob¬ 
jects  overlapping  at  different  ranges  (e.g.,  multiple  layers 
of  tree  limbs). 

The  filter  successfully  utilized  inertial  measurements  to 
predict  and  constrain  the  image  correspondence  search  dur¬ 
ing  the  entire  profile.  In  return,  the  feature  correspondence 
updated  and  corrected  the  inertial  measurement  errors  and 
significantly  reduced  the  resulting  drift  in  the  navigation 
solution.  Over  the  four-minute  non-stationary  profile,  the 
navigation  errors  were  estimated  to  be  less  than  1  m  in  the 
horizontal  plane  and  less  than  3  m  in  the  vertical.  Typi¬ 
cal  free-inertial  performance  for  this  inertial  sensor  is  esti¬ 
mated  to  be  on  the  order  of  hundreds  of  meters  horizonal. 
The  unstable  nature  of  the  vertical  channel  would  require 
external  aiding  in  order  to  maintain  stability.  These  initial 
results  clearly  show  the  benefits  of  the  described  method. 

The  second  profile  consisted  of  a  closed  path  in  an  indoor 
environment.  The  path  began  and  ended  at  the  same  loca¬ 
tion  and  orientation  in  the  Advanced  Navigation  Technol¬ 
ogy  Center  laboratory.  As  in  the  previous  profile,  the  data 


Figure  6:  Sample  image  from  indoor  data  collection.  The 
indoor  data  collection  presented  the  filter  with  man-made 
features  in  an  office  environment.  The  crosses  and  ellipses 
indicate  the  locations  and  uncertainty  of  currently  tracked 
landmarks. 

collection  began  with  a  10-minute  stationary  alignment  pe¬ 
riod.  After  the  alignment  period,  the  sensor  was  moved  in 
a  10-minute  loop  around  the  hallways  of  the  building.  In 
contrast  to  the  previous  profile,  the  sensor  was  pointed  pri¬ 
marily  in  the  direction  of  travel.  No  prior  knowledge  was 
provided  to  the  algorithm  regarding  the  location  of  features 
or  structure  of  the  environment.  A  sample  image  from  the 
indoor  profile  is  shown  in  Figure  6. 

The  indoor  profile  presented  the  algorithm  with  different 
challenges  from  a  feature  tracking  perspective.  The  indoor 
environment  consisted  of  repetitive,  visually  identical  fea¬ 
tures  (e.g.,  floor  tiles,  lights,  etc.),  which  could  easily  cause 
confusion  for  the  feature  tracking  algorithm.  In  addition, 
reflections  from  windows  and  other  shiny  surfaces  would 
not  be  interpreted  properly  by  the  filter  and  could  poten¬ 
tially  result  in  navigation  errors.  Finally,  the  lower  aver¬ 
age  light  intensity  levels  and  large  areas  with  poor  contrast 
(e.g.,  smooth,  featureless  walls)  presented  a  relatively  stark 
feature  space. 

As  with  the  previous  profile,  the  filter  performed  well.  The 
filter’s  estimate  of  the  trajectory  was  overlayed  on  a  floor 
plan  of  the  building  in  Figure  7  along  with  the  inertial- 
only  trajectory  calculated  using  the  Novatel  Black  Dia¬ 
mond  System  and  an  image-aided  inertial  trajectory  with 
stochastic  constraints  disabled.  The  estimated  trajectory 
corresponds  well  to  the  building’s  hallways.  The  inertial- 
only  trajectory  quickly  develops  large  errors,  even  though 
the  Novatel  filter  is  applying  an  altitude  hold  correction 
and  using  a  vehicle  motion  model  in  an  attempt  to  con¬ 
strain  the  inertial  drift.  With  stochastic  constraints  pur- 


Figure  7:  Estimated  path  from  indoor  data  collection.  The 
filter’s  estimate  of  the  path  (indicated  by  the  solid  line) 
agrees  well  with  the  known  floor  plan.  The  inertial-only 
best  estimate  of  trajectory  (indicated  by  the  dashed  line) 
and  optical-inertial  with  stochastic  constraints  disabled 
(indicated  by  the  dotted  line)  show  large  errors  in  position 
and  heading. 


Figure  9:  Enhanced  detail  of  the  start/stop  area  illustrat¬ 
ing  the  estimated  trajectory  and  feature  locations.  The  dif¬ 
ference  between  the  estimated  start  and  stop  location  illus¬ 
trates  the  accumulated  position  error. 

which  utilize  the  natural  strengths  of  each  sensor  in  a  syn¬ 
ergistic  manner.  Results  from  two  data  collection  scenarios 
show  the  effectiveness  of  the  stochastic  projection  method 
for  tightly-integrating  optical  and  inertial  sensors.  The  fil¬ 
ter  successfully  utilized  inertial  measurements  to  enhance 
feature  tracking  while  simultaneously  correcting  for  iner¬ 
tial  errors.  These  tests  demonstrate  the  viability  of  our  ap¬ 
proach  for  navigation  without  external  signals. 


posely  disabled,  the  trajectory  estimate  quickly  diverges 
due  to  false  correspondence  matches.  This  illustrates  the 
catastrophic  effects  of  incorporating  false  updates  into  an 
Extended  Kalman  Filter  with  inertial  feedback  and  high¬ 
lights  the  inherent  strength  of  applying  robust  correspon¬ 
dence  methods,  and  in  particular  stochastic  constraints. 

The  filter’s  estimated  trajectory  and  estimated  location  of 
features  used  for  tracking  are  shown  in  Figure  8.  More 
detail  of  the  start/stop  area  is  shown  in  Figure  9.  The  dif¬ 
ference  in  the  estimated  start  and  stop  locations  shows  the 
accumulated  errors  in  the  filter  over  the  path.  Over  the  10- 
minute  profile,  the  navigation  errors  were  less  than  1  m  in 
the  horizontal  plane  and  less  than  3  m  in  the  vertical.  Again, 
this  was  a  significant  improvement  over  the  Novatel  free- 
inertial  performance.  The  results  are  particularly  impres¬ 
sive  as  the  solution  was  calculated  using  an  online  algo¬ 
rithm,  with  only  raw  inertial  and  image  data.  No  a-priori 
knowledge  of  the  environment  was  provided  to  the  system. 


DISCLAIMER 

The  views  expressed  in  this  article  are  those  of  the  au¬ 
thor  and  do  not  reflect  the  official  policy  or  position  of  the 
United  States  Air  Force,  Department  of  Defense,  or  the  U.S 
Government. 


CONCLUSIONS 

In  this  paper,  an  algorithm  is  presented  which  integrates 
inertial  and  optical  measurements  to  provide  an  enhanced 
navigation  solution.  Stochastic  algorithms  are  applied 


a)  Entire  path  estimate 


b)  First  half  of  estimated  path  with  tracked  feature  locations 


c)  Second  half  of  estimated  path  with  tracked  feature 


d)  Detail  of  start/stop  area  with  tracked  feature  locations 
locations 


Figure  8:  Estimated  path  and  feature  locations  from  indoor  data  collection.  Pane  (a)  shows  the  entire  path  estimate.  Pane  (b) 
shows  the  first  half  of  the  path  along  with  the  estimated  location  of  the  features  (indicated  by  x  symbols).  Pane  (c)  shows  the 
last  half  of  the  path  and  estimated  feature  locations.  Pane  (d)  provides  detail  of  the  start/stop  area. 
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